#include <PS2X_lib.h>  //for v1.6

/******************************************************************
   set pins connected to PS2 controller:
     - 1e column: original
     - 2e colmun: Stef?
   replace pin numbers by the ones you use
 ******************************************************************/
#define PS2_DAT        12  //14    
#define PS2_CMD        11  //15
#define PS2_SEL        10  //16
#define PS2_CLK        13  //17

/******************************************************************
   select modes of PS2 controller:
     - pressures = analog reading of push-butttons
     - rumble    = motor rumbling
   uncomment 1 of the lines for each mode selection
 ******************************************************************/
//#define pressures   true
#define pressures   false
//#define rumble      true
#define rumble      false

PS2X ps2x; // create PS2 Controller Class

//right now, the library does NOT support hot pluggable controllers, meaning
//you must always either restart your Arduino after you connect the controller,
//or call config_gamepad(pins) again after connecting the controller.

int error = 0;
byte type = 0;
byte vibrate = 0;

// Reset func
void (* resetFunc) (void) = 0;

#include <Servo.h>                       //调用“Servo”库，调制无刷电机PWM输出
Servo ESC;                               //无刷电机命名为“ESC”
String throttle;                         //定义接收字符串为“throttle”
const int buzzer = 2;                    // 定义喇叭引脚为2号数字引脚
const int ledPin = 3;                  // 定义喇叭引脚为3号数字引脚
int mTime;
bool heart = 0;

void setup()                             //初始化参数
{
  ESC.attach(9);                        // 定义无刷电机引脚为9号数字引脚，可以输出PWM
  Serial.begin(9600);                  //设置串口通讯的波特率为9600

  ESC.writeMicroseconds(1000);            //无刷电机初始为低电平
  pinMode(ledPin, OUTPUT);             //前灯继电器输出
  pinMode(buzzer, OUTPUT);             //喇叭输出
  error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
  if (error == 0)
  {
    Serial.print("Found Controller, configured successful ");
    Serial.print("pressures = ");
    if (pressures)
      Serial.println("true ");
    else
      Serial.println("false");
    Serial.print("rumble = ");
    if (rumble)
      Serial.println("true)");
    else
      Serial.println("false");
    Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
    Serial.println("holding L1 or R1 will print out the analog stick values.");
    Serial.println("Note: Go to www.billporter.info for updates and to report bugs.");
  }
  else if (error == 1)
    Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");

  else if (error == 2)
    Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");

  else if (error == 3)
    Serial.println("Controller refusing to enter Pressures mode, may not support it. ");



}
void loop() {
  /* You must Read Gamepad to get new values and set vibration values
     ps2x.read_gamepad(small motor on/off, larger motor strenght from 0-255)
     if you don't enable the rumble, use ps2x.read_gamepad(); with no values
     You should call this at least once a second
  */
  if (error == 1) { //skip loop if no controller found
    resetFunc();
  }


  else { //DualShock Controller
    ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed

    if (ps2x.ButtonPressed(PSB_START))        //will be TRUE as long as button is pressed
      Serial.println("Start is being held");
    if (ps2x.ButtonPressed(PSB_SELECT))
    {
      Serial.println("Reset");
      digitalWrite(buzzer, LOW);
      digitalWrite(ledPin, LOW);
      ESC.writeMicroseconds(1000);
    }

    if (ps2x.ButtonPressed(PSB_L3))
    {
      digitalWrite(ledPin, HIGH);
      Serial.println(" turn on");
      throttle = "0";
    }
    if (ps2x.ButtonPressed(PSB_R3))
    {
      digitalWrite(ledPin, LOW);
      Serial.println(" turn off");
      throttle = "0";
    }
    if (ps2x.ButtonPressed(PSB_L2))
    {
      int times = 5;
      Serial.println(" buzzer on");
      for (int i = 0; i <= times; i++)
      {
        digitalWrite(buzzer, HIGH);
        delay(300);
        digitalWrite(buzzer, LOW);
        delay(200);
      }
      throttle = "0";
    }
    if (ps2x.ButtonPressed(PSB_R2))
    {
      int times = 2;
      Serial.println(" buzzer off");
      for (int i = 0; i <= times; i++)
      {
        digitalWrite(buzzer, HIGH);
        delay(100);
        digitalWrite(buzzer, LOW);
        delay(200);
      }
      throttle = "0";
    }


    if (ps2x.ButtonPressed(PSB_CIRCLE))              //will be TRUE if button was JUST pressed
    {
      int i = ESC.readMicroseconds();
      if (i <= 1350)
        for (i; i <= 1350; i++)
        {
          ESC.writeMicroseconds(i);
          delay(2);
        }
      else
        for (i; i >= 1350; i--)
        {
          ESC.writeMicroseconds(i);
          delay(2);
        }
      Serial.println(" speed 1");
      throttle = "0";
    }
    if (ps2x.ButtonPressed(PSB_CROSS))              //will be TRUE if button was JUST pressed OR released
    {
      int i = ESC.readMicroseconds();
      if (i <= 1550)
        for (i; i <= 1550; i++)
        {
          ESC.writeMicroseconds(i);
          delay(2);
        }
      else
        for (i; i >= 1550; i--)
        {
          ESC.writeMicroseconds(i);
          delay(2);
        }
      Serial.println(" speed 2");
      throttle = "0";
    }
    if (ps2x.ButtonPressed(PSB_SQUARE))             //will be TRUE if button was JUST released
    {
      int i = ESC.readMicroseconds();
      if (i <= 1750)
        for (i; i <= 1750; i++)
        {
          ESC.writeMicroseconds(i);
          delay(2);
        }
      else
        for (i; i >= 1750; i--)
        {
          ESC.writeMicroseconds(i);
          delay(2);
        }
      Serial.println(" speed 3");
      throttle = "0";
    }
    if (ps2x.ButtonPressed(PSB_TRIANGLE))
    {
      int i = ESC.readMicroseconds();
      if (i <= 1950)
        for (i; i <= 1950; i++)
        {
          ESC.writeMicroseconds(i);
          delay(2);
        }
      else
        for (i; i >= 1950; i--)
        {
          ESC.writeMicroseconds(i);
          delay(2);
        }
      Serial.println(" speed 4");
      throttle = "0";
    }
    if (ps2x.ButtonPressed(PSB_L1) || ps2x.ButtonPressed(PSB_R1))
    { //print stick values if either is TRUE
      Serial.println(" stop");
      digitalWrite(buzzer, LOW);
      digitalWrite(ledPin, LOW);
      ESC.writeMicroseconds(1000);

    }
  }
  delay(50);

}
